#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);
    const double degree = M_PI/180;

    // robot state
    double inc=0, angle=0;
    double head = 0, head_inc=0.005; //degree
    double arm = 0, arm_inc=0.005; //meter
    double l_gripper = 0, l_gripper_inc=0.005; //degree
    double r_gripper = 0, r_gripper_inc=0.005; //degree
    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";
    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(4);
        joint_state.position.resize(4);
        joint_state.name[0] ="head_swivel";
        joint_state.position[0] = head;
        joint_state.name[1] ="gripper_extension";
        joint_state.position[1] = arm;
        joint_state.name[2] ="left_gripper_joint";
        joint_state.position[2] = l_gripper;
        joint_state.name[3] ="right_gripper_joint";
        joint_state.position[3] = r_gripper;
          // update transform
        // (moving in a circle with radius=1)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle);
        odom_trans.transform.translation.y = sin(angle);
        odom_trans.transform.translation.z = 0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);
              // Create new robot state
        head += degree; //head rotation
        if(arm < -0.38 || arm > 0) //arm extendable
        	arm_inc *= -1;
        arm += arm_inc;
        if(l_gripper < 0 || l_gripper > 0.55) // left gripper opening-closing
        	l_gripper_inc *= -1;
        l_gripper += l_gripper_inc;
        if(r_gripper < 0 || r_gripper > 0.55) // right gripper opening-closing
        	r_gripper_inc *= -1;
        r_gripper += r_gripper_inc;
        angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }

    return 0;
}